home *** CD-ROM | disk | FTP | other *** search
/ C/C++ Users Group Library 1996 July / C-C++ Users Group Library July 1996.iso / listings / v_10_11 / 1011058a < prev    next >
Encoding:
Text File  |  1992-06-17  |  9.4 KB  |  329 lines
  1. /*  CORDIC.C : Demonstrates the integer-based CORDIC
  2.  *   system for calculating sines and cosines. The
  3.  *   vertices of a regular hexagon are calculated using
  4.  *   CORDIC trig and the hexagon itself is rotated.
  5.  *   Make in Borland C++'s internal environment.
  6.  *   (c) 1991 Michael Bertrand.
  7.  */
  8.  
  9. #include <stdio.h>     /* printf */
  10. #include <math.h>      /* sqrt, atan */
  11. #include <conio.h>     /* getch */
  12. #include <stdlib.h>    /* */
  13. #include <graphics.h>  /* BGI */
  14.  
  15. typedef struct
  16. {
  17.   int x;
  18.   int y;
  19. } POINT;
  20.  
  21. typedef unsigned int WORD;
  22.  
  23. void CalcHexPtsCORDIC(POINT center, POINT vertex);
  24. void DrawHexagon(POINT center, POINT vertex);
  25. void DrawCross(POINT pt, int colr);
  26. void SinCosSetup(void);
  27. void SinCos(WORD theta, int *sin, int *cos);
  28.  
  29. #define ESC   0x1B
  30.  
  31. /* Quadrants for angles. */
  32. #define QUAD1 1
  33. #define QUAD2 2
  34. #define QUAD3 3
  35. #define QUAD4 4
  36.  
  37. /* NBITS is number of bits for CORDIC units. */
  38. #define NBITS 14
  39.  
  40. /* NUM_PTS is number of vertices in polygon. */
  41. #define NUM_PTS 6
  42.  
  43. int  ArcTan[NBITS];      /* angles for rotation */
  44. int  xInit;              /* initial x projection */
  45. WORD CordicBase;         /* base for CORDIC units */
  46. WORD HalfBase;           /* CordicBase / 2 */
  47. WORD Quad2Boundary;      /* 2 * CordicBase */
  48. WORD Quad3Boundary;      /* 3 * CordicBase */
  49. POINT HexPts[NUM_PTS+1]; /* calculated poly points */
  50.  
  51. void main(void)
  52. {
  53.   int   driver;   /* for initgraph() */
  54.   int   mode;     /* for initgraph() */
  55.   WORD  theta;    /* CORDIC angle */
  56.   int   sine;     /* sine of CORDIC angle */
  57.   int   cosine;   /* cosine of CORDIC angle */
  58.   POINT center;   /* center of hexagon */
  59.   POINT vertex;   /* hexagon's original base vertex */
  60.   POINT vertex1;  /* hexagon's changing base vertex */
  61.   POINT del;      /* vertex - center (radial spoke) */
  62.   int   radius;   /* radius of circumscribing circle */
  63.  
  64.   driver = VGA;      /* for initgraph() */
  65.   mode   = VGAHI;    /* mode 0x12 : 640x480 16 color */
  66.  
  67.   if (registerbgidriver(EGAVGA_driver) < 0)
  68.     {
  69.     printf("couldn't find VGA driver");  return;
  70.     }
  71.   initgraph(&driver, &mode, NULL);
  72.  
  73.   printf("Press ENTER to rotate, ESC to quit.");
  74.  
  75.   center.x = 320;  center.y = 240;
  76.   vertex.x = 470;  vertex.y = 240;
  77.   radius = vertex.x - center.x;
  78.   /* Calculate the radial spoke : vertex - center. */
  79.   del.x = vertex.x - center.x;
  80.   del.y = vertex.y - center.y;
  81.  
  82.   setwritemode(XOR_PUT);
  83.  
  84.   /* Draw circumscribing circle. */
  85.   setcolor(RED);
  86.   circle(center.x, center.y, radius);
  87.  
  88.   /* Draw small cross at center. */
  89.   DrawCross(center, YELLOW);
  90.   setcolor(WHITE);
  91.  
  92.   /* Setup CORDIC system, initialize theta = 0. */
  93.   SinCosSetup();
  94.   theta = 0;
  95.  
  96.   /* Draw initial hexagon. */
  97.   DrawHexagon(center, vertex1 = vertex);
  98.  
  99.   /* Rotate hexagon. vertex is fixed; vertex1 rotates
  100.    * clockwise around vertex in increments of 650
  101.    * CORDIC units (3.57 deg). CORDIC sines/cosines are
  102.    * used to find vertex1. DrawHexagon() also uses
  103.    * CORDIC sines/cosines to calculate the remaining
  104.    * vertices for each hexagon so they can be drawn.
  105.    */
  106.   while (getch() != ESC)
  107.     {
  108.     /* Erase last hexagon. */
  109.     DrawHexagon(center, vertex1);
  110.     /* Inc theta by 650 CORDIC units (3.57 deg). */
  111.     theta += 650;
  112.     SinCos(theta, &sine, &cosine);
  113.     /* Calc new vertex by rotating around center. */
  114.     vertex1.x = (int)
  115.        (((long) del.x * cosine - (long) del.y * sine +
  116.        HalfBase) >> NBITS) + center.x;
  117.     vertex1.y = (int)
  118.        (((long) del.x * sine + (long) del.y * cosine +
  119.        HalfBase) >> NBITS) + center.y;
  120.     /* Draw new hexagon. */
  121.     DrawHexagon(center, vertex1);
  122.     }
  123.  
  124.   closegraph();
  125. }
  126.  
  127. void CalcHexPtsCORDIC(POINT center, POINT vertex)
  128. /*
  129.   USE:  Calc array of hex vertices using CORDIC calcs.
  130.   IN:   center = center of hexagon.
  131.         vertex = one of the hexagon vertices
  132.   NOTE: Loads global array HexPoints[] with other 5
  133.    vertices of regular hexagon. Uses CORDIC routines
  134.    for trig and long integer calculations.
  135. */
  136. {
  137.   int   sine;     /* sine of central angle */
  138.   int   cosine;   /* cosine of central angle */
  139.   int   corner;   /* index for vertices of polygon */
  140.   POINT del;      /* vertex - center (radial spoke) */
  141.  
  142.   /* 60 deg = 10923 CORDIC units. */
  143.   SinCos(10923, &sine, &cosine);
  144.  
  145.   /* Set initial and final point to incoming vertex. */
  146.   HexPts[0].x = HexPts[NUM_PTS].x = vertex.x;
  147.   HexPts[0].y = HexPts[NUM_PTS].y = vertex.y;
  148.  
  149.   /* Go clockwise around circle to calc hex points. */
  150.   for (corner = 1; corner < NUM_PTS; corner++)
  151.     {
  152.     /* Calculate the radial spoke : vertex - center. */
  153.     del.x = vertex.x - center.x;
  154.     del.y = vertex.y - center.y;
  155.     /* calc new vertex by rotating around center. */
  156.     vertex.x = (int)
  157.        (((long) del.x * cosine - (long) del.y * sine +
  158.        HalfBase) >> NBITS) + center.x;
  159.     vertex.y = (int)
  160.        (((long) del.x * sine + (long) del.y * cosine +
  161.        HalfBase) >> NBITS) + center.y;
  162.     /* Store new vertex in array. */
  163.     HexPts[corner].x = vertex.x;
  164.     HexPts[corner].y = vertex.y;
  165.     }
  166. }
  167.  
  168. void DrawHexagon(POINT center, POINT vertex)
  169. /*
  170.   USE:  Draw Hexagon given center and one vertex.
  171.   IN:   center = center of hexagon.
  172.         vertex = one of the hexagon vertices
  173.   NOTE: Call CalcHexPtsCORDIC() to load global array
  174.    HexPts[] with hexagon vertices.
  175. */
  176. {
  177.   CalcHexPtsCORDIC(center, vertex);
  178.   drawpoly(NUM_PTS+1, (int far *)HexPts);
  179. }
  180.  
  181. void DrawCross(POINT pt, int colr)
  182. /*
  183.   USE:  Draw cross on screen at pt with given color.
  184. */
  185. {
  186.   int oldColor;
  187.  
  188.   setwritemode(COPY_PUT);
  189.   oldColor = getcolor();
  190.   setcolor(colr);
  191.   moveto(pt.x - 2, pt.y); lineto(pt.x + 2, pt.y);
  192.   moveto(pt.x, pt.y - 2); lineto(pt.x, pt.y + 2);
  193.   setcolor(oldColor);
  194.   setwritemode(XOR_PUT);
  195. }
  196.  
  197. void SinCosSetup(void)
  198. /*
  199.   USE : Load globals used by SinCos().
  200.   OUT : Loads globals used in SinCos() :
  201.    CordicBase    = base for CORDIC units
  202.    HalfBase      = Cordicbase / 2
  203.    Quad2Boundary = 2 * CordicBase
  204.    Quad3Boundary = 3 * CordicBase
  205.    ArcTan[]      = the arctans of 1/(2^i)
  206.    xInit         = initial value for x projection
  207.   NOTE: Must be called once only to initialize before
  208.    calling SinCos(). xInit is sufficiently less than
  209.    CordicBase to exactly compensate for the expansion
  210.    in each rotation.
  211. */
  212. {
  213.   int i;        /* to index ArcTan[] */
  214.   double f;     /* to calc initial x projection */
  215.   long powr;    /* powers of 2 up to 2^(2*(NBITS-1)) */
  216.  
  217.   CordicBase = 1 << NBITS;
  218.   HalfBase = CordicBase >> 1;
  219.   Quad2Boundary = CordicBase << 1;
  220.   Quad3Boundary = CordicBase + Quad2Boundary;
  221.  
  222.   /* ArcTan's are diminishingly small angles. */
  223.   powr = 1;
  224.   for (i = 0; i < NBITS; i++)
  225.     {
  226.     ArcTan[i] = (int)
  227.       (atan(1.0/powr)/(M_PI/2)*CordicBase + 0.5);
  228.     powr <<= 1;
  229.     }
  230.  
  231.   /* xInit is initial value of x projection to comp-
  232.    * ensate for expansions.  f = 1/sqrt(2/1 * 5/4 * ...
  233.    * Normalize as an NBITS binary fraction (multiply by
  234.    * CordicBase) and store in xInit. Get f = 0.607253
  235.    * and xInit = 9949 = 0x26DD for NBITS = 14.
  236.    */
  237.   f = 1.0;
  238.   powr = 1;
  239.   for (i = 0; i < NBITS; i++)
  240.     {
  241.     f = (f * (powr + 1)) / powr;
  242.     powr <<= 2;
  243.     }
  244.   f = 1.0/sqrt(f);
  245.   xInit = (int) (CordicBase * f + 0.5);
  246. }
  247.  
  248. void SinCos(WORD theta, int *sin, int *cos)
  249. /*
  250.   USE : Calc sin and cos with integer CORDIC routine.
  251.   IN  : theta = incoming angle (in CORDIC angle units)
  252.   OUT : sin = ptr to sin (in CORDIC fixed point units)
  253.         cos = ptr to cos (in CORDIC fixed point units)
  254.   NOTE: The incoming angle theta is in CORDIC angle
  255.    units, which subdivide the circle into 64K parts,
  256.    with 0 deg = 0, 90 deg = 16384 (CordicBase), 180 deg
  257.    = 32768, 270 deg = 49152, etc. The calculated sine
  258.    and cosine are in CORDIC fixed point units : an int
  259.    considered as a fraction of 16384 (CordicBase).
  260. */
  261. {
  262.   int quadrant;  /* quadrant of incoming angle */
  263.   int z;         /* incoming angle moved to 1st quad */
  264.   int i;         /* to index rotations : one per bit */
  265.   int x, y;      /* projections onto axes */
  266.   int x1, y1;    /* projections of rotated vector */
  267.  
  268.   /* Determine quadrant of incoming angle, translate to
  269.    * 1st quadrant. Note use of previously calculated
  270.    * values CordicBase, etc. for speed.
  271.    */
  272.   if (theta < CordicBase)
  273.     {
  274.     quadrant = QUAD1;
  275.     z = (int) theta;
  276.     }
  277.   else if (theta < Quad2Boundary)
  278.     {
  279.     quadrant = QUAD2;
  280.     z = (int) (Quad2Boundary - theta);
  281.     }
  282.   else if (theta < Quad3Boundary)
  283.     {
  284.     quadrant = QUAD3;
  285.     z = (int) (theta - Quad2Boundary);
  286.     }
  287.   else
  288.     {
  289.     quadrant = QUAD4;
  290.     z = - ((int) theta);
  291.     }
  292.  
  293.   /* Initialize projections. */
  294.   x = xInit;
  295.   y = 0;
  296.  
  297.   /* Negate z, so same rotations taking angle z to 0
  298.    * will take (x, y) = (xInit, 0) to (*cos, *sin).
  299.    */
  300.   z = -z;
  301.  
  302.   /* Rotate NBITS times. */
  303.   for (i = 0; i < NBITS; i++)
  304.     {
  305.     if (z < 0)
  306.       {
  307.       /* Counter-clockwise rotation. */
  308.       z += ArcTan[i];
  309.       y1 = y + (x >> i);
  310.       x1 = x - (y >> i);
  311.       }
  312.     else
  313.       {
  314.       /* Clockwise rotation. */
  315.       z -= ArcTan[i];
  316.       y1 = y - (x >> i);
  317.       x1 = x + (y >> i);
  318.       }
  319.  
  320.     /* Put new projections into (x,y) for next go. */
  321.     x = x1;
  322.     y = y1;
  323.     }  /* for i */
  324.  
  325.   /* Attach signs depending on quadrant. */
  326.   *cos = (quadrant==QUAD1 || quadrant==QUAD4) ? x : -x;
  327.   *sin = (quadrant==QUAD1 || quadrant==QUAD2) ? y : -y;
  328. }
  329.